package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Utility;

public class EncoderFeedback {
	private Encoder flencoder;
	private Encoder frencoder;
	private Encoder rlencoder;
	private Encoder rrencoder;
	private double  flencoderdist;
	private double  frencoderdist;
	private double  rlencoderdist;
	private double  rrencoderdist;
	private double  dflencoderdist;
	private double  dfrencoderdist;
	private double  drlencoderdist;
	private double  drrencoderdist;
	
	private long    time;
	private double  dt;
	
	//static private final double distance_per_pulse = 8*Math.PI/360.0;
	static private final double dist_per_rot   = 8*Math.PI;
	//static private final double rot_multiplier = -0.025;
	static private final double rot_multiplier = -0.021;
	
	EncoderFeedback() {
		flencoder = new Encoder(4,3);
		frencoder = new Encoder(7,8);
		rlencoder = new Encoder(2,1);
		rrencoder = new Encoder(5,6);
		
		/*flencoder.setDistancePerPulse(distance_per_pulse);
		frencoder.setDistancePerPulse(distance_per_pulse);
		rlencoder.setDistancePerPulse(distance_per_pulse);
		rrencoder.setDistancePerPulse(distance_per_pulse);*/
		
		flencoder.setDistancePerPulse(dist_per_rot/360.0);
		frencoder.setDistancePerPulse(dist_per_rot/250.0);
		rlencoder.setDistancePerPulse(dist_per_rot/360.0);
		rrencoder.setDistancePerPulse(dist_per_rot/250.0);
		
		flencoder.start();
		frencoder.start();
		rlencoder.start();
		rrencoder.start();
		
		flencoderdist = flencoder.getDistance();
		frencoderdist = frencoder.getDistance();
		rlencoderdist = rlencoder.getDistance();
		rrencoderdist = rrencoder.getDistance();
		
		time = Utility.getFPGATime();
		dt   = 0.0;
		
		dflencoderdist = 0.0;
		dfrencoderdist = 0.0;
		drlencoderdist = 0.0;
		drrencoderdist = 0.0;
	}
	
	double getX() {
		return (flencoderdist - frencoderdist - rlencoderdist + rrencoderdist) / 4.0;
	}
	
	double getY() {
		return (flencoderdist + frencoderdist + rlencoderdist + rrencoderdist) / 4.0;
	}
	
	void updateErr(Vector err) {
		err.setX(err.getX() - (dflencoderdist - dfrencoderdist -
		                       drlencoderdist + drrencoderdist)/4.0);
		err.setY(err.getY() - (dflencoderdist + dfrencoderdist +
		                       drlencoderdist + drrencoderdist)/4.0);
	}
	
	void updateVel(Vector vel) {
		vel.setX((dflencoderdist - dfrencoderdist -
		          drlencoderdist + drrencoderdist) / (4.0 * dt));
		vel.setY((dflencoderdist + dfrencoderdist +
		          drlencoderdist + drrencoderdist) / (4.0 * dt));
	}
	
	void update() {
		dt    = ((double) (Utility.getFPGATime() - time)) / 1000000.0;
		time += dt * 1000000.0;
		
		dflencoderdist = flencoder.getDistance() - flencoderdist;
		dfrencoderdist = frencoder.getDistance() - frencoderdist;
		drlencoderdist = rlencoder.getDistance() - rlencoderdist;
		drrencoderdist = rrencoder.getDistance() - rrencoderdist;
		
		flencoderdist += dflencoderdist;
		frencoderdist += dfrencoderdist;
		rlencoderdist += drlencoderdist;
		rrencoderdist += drrencoderdist;
	}
	
	double getAngularRate() {
		return rot_multiplier *
		       (dflencoderdist-dfrencoderdist+drlencoderdist-drrencoderdist)/dt;
	}
}